#include "MoYuMath.h"
#include <iostream>

using namespace MoYuMath;

void Output(Matrix3x3& mat) {
	for (size_t i = 0; i < 3; i++)
	{
		for (size_t j = 0; j < 3; j++)
		{
			std::cout << mat[i][j];
			if (j != 2)
				std::cout << ", ";
		}
		std::cout << std::endl;
	}
	std::cout << std::endl;
}

void Output(Matrix4x4& mat) {
	for (size_t i = 0; i < 4; i++)
	{
		for (size_t j = 0; j < 4; j++)
		{
			std::cout << mat[i][j];
			if(j != 3)
				std::cout << ", ";
		}
		std::cout << std::endl;
	}
}

void Output(Vector3& vec) {
	std::cout << vec.x << ", " << vec.y << ", " << vec.z << std::endl;
}

void Output(Quaternion& quat) {
	std::cout << quat.x << ", " << quat.y << ", " << quat.z << ", " << quat.w << std::endl;
}

void Output(float val) {
	std::cout << val<< std::endl;
}


void OutputLine(std::string testStr = "") {
	std::cout << testStr << std::endl;
}

void TestRotation1()
{
	Matrix3x3 rotMat = Matrix3x3::fromYawPitchRoll(0, Math_PI * 0.5f, 0);
	Quaternion rotQuat = Quaternion::fromRotationMatrix(rotMat);
	Quaternion deltaQuat = Quaternion::fromYawPitchRoll(0, Math_PI * 0.5f, 0);

	Vector3 rotTest0 = Vector3(0, 1, 0);
	Vector3 rotTest1 = rotMat * rotTest0;
	Vector3 rotTest2 = rotQuat * rotTest0;
	Vector3 rotTest3 = deltaQuat * rotTest0;

	Output(rotTest0);

	OutputLine("========");

	Output(rotMat);
	OutputLine();
	Output(rotTest1);
	OutputLine();
	
	OutputLine("========");

	Output(rotQuat);
	OutputLine();
	Output(rotTest2);
	OutputLine();

	OutputLine("========");

	Output(deltaQuat);
	OutputLine();
	Output(rotTest3);
	OutputLine();

	OutputLine("========");

	Vector3 matEuler = rotMat.toEuler();
	Output(matEuler * Math_fRad2Deg);
	OutputLine();

	OutputLine("========");

	Vector3 oriEuler = rotMat.inverse().toEuler();
	Output(oriEuler * Math_fRad2Deg);
	OutputLine();

	OutputLine("========");

	Vector3 quatEuler = rotQuat.toEuler();
	Output(quatEuler * Math_fRad2Deg);
	OutputLine();

	OutputLine("========");

	Quaternion quat2 = Quaternion::fromYawPitchRoll(0, 0, 0);
	Quaternion quatMiddle = Quaternion::slerp(quat2, rotQuat, 0.5f);
	float quatAngle = Quaternion::angle(quat2, quatMiddle);
	Output(quatAngle * Math_fRad2Deg);

	OutputLine("========");

	Vector3 eulerMiddleMat = Matrix3x3::fromQuaternion(quatMiddle).toEuler();
	Output(eulerMiddleMat * Math_fRad2Deg);

	OutputLine("========");

	Vector3 eulerMiddel = quatMiddle.toEuler();
	Output(eulerMiddel * Math_fRad2Deg);
	OutputLine();
}

void TestWorld()
{
	Vector3 position = Vector3(1, 1, 0);
	Vector3 forward = Vector3::Right;
	Vector3 up = Vector3::Up;
	Matrix4x4 localToWorld = Matrix4x4::createWorld(position, forward, up);
	Output(localToWorld);
	OutputLine();

	Vector3 localPos = Vector3(0.1, 2, 1);
	Vector3 worldPos = localToWorld * localPos;

	Output(localPos);
	OutputLine();

	Output(worldPos);
	OutputLine();

}

void main(void) {
	
	//Vector3 position = Vector3(0, 0, 0);
	//Vector3 scale = Vector3::One;
	//Quaternion rotation = Quaternion::fromAxisAngle(Vector3(0, 1, 0), Math_PI * 0.5f);

	//Matrix4x4 localToWorld = Matrix4x4::makeTransform(position, scale, rotation);
	//Output(localToWorld);

	//Vector3 localPos = Vector3(0.2, 0.3, 1);
	//Vector3 worldPos = localToWorld * localPos;

	//Output(localPos);
	//Output(worldPos);

	//Vector3 worldPosRot = rotation * localPos;
	//Output(worldPosRot);

	//Quaternion rotationInv = Quaternion::inverse(rotation);
	//Vector3 localPosBack = rotationInv * worldPos;
	//Output(localPosBack);

	TestRotation1();
	//TestWorld();

	return;
}
